#include "RGBMessage.h"

byte OFF[3]={
  0,0,0};
byte ON[3]={
  255, 255, 255};


const int LEDS = 2;

const byte JOBS_PINS[LEDS][3]= {
  {
    11,10,9      }
  , {
    6,5,3        }
};

const float SPEED=0.07;

const int LED_UPDATE_MS = 40;


struct buildingState {

  byte rgbPins[3] ;                // 3 pins connected to R,G,B  LED
  byte rgbValue[3] ;                // previous rgbValue of the LED
  long previousMillis ;        // will store last time LED was updated
  long interval ;           // interval at which to blink (milliseconds)
  // for blinking state
  boolean isOn;
  float index;            
  byte rgbCurrent[3] ;

  void (*currentAction)(buildingState &);

};


void building(buildingState &state) {

  long current=millis();

  // here is where you'd put code that needs to be running all the time.

  // check to see if it's time to blink the LED; that is, is the difference
  // between the current time and last time we blinked the LED bigger than
  // the interval at which we want to blink the LED.
  if (current - state.previousMillis > state.interval) {
    state.previousMillis = current;   // remember the last time we blinked the LED
    state.index+=SPEED;

    float factor= sin(state.index)/2+0.5;
    state.rgbCurrent[0]= byte(factor* state.rgbValue[0]);
    state.rgbCurrent[1]= byte(factor* state.rgbValue[1]);
    state.rgbCurrent[2]= byte(factor* state.rgbValue[2]);

    writeToLED(state.rgbPins, state.rgbCurrent);
  }

  if (state.index > 3.3028235E+38) {
    state.index=0;
  }

}


void inactive(buildingState &state) {

  if (!state.isOn) 
  {
    writeToLED(state.rgbPins, OFF );
    state.isOn=true;
  }

}

void pending(buildingState &state) {

  if (!state.isOn) 
  {
    writeToLED(state.rgbPins, state.rgbValue );
    state.isOn=true;
  }

}

void setArray(byte to[3], const byte from[3]) {
  to[0]=from[0]; 
  to[1]=from[1];
  to[2]=from[2];
}

void writeToLED(byte rgbPins[3], byte rgbrgbValue[3]) {
  /*
  if (rgbPins[0]==6) {
    Serial.print("pins:");
    Serial.print(int(rgbPins[0]));
    Serial.print("=");
    Serial.println(int(rgbrgbValue[0]));

    Serial.print(int(rgbPins[1]));
    Serial.print("=");
    Serial.println(int(rgbrgbValue[1]));

    Serial.print(int(rgbPins[2]));
    Serial.print("=");
    Serial.println(int(rgbrgbValue[2]));

    Serial.println("======");
  }
  */
  analogWrite(rgbPins[0], rgbrgbValue[0]);
  analogWrite(rgbPins[1], rgbrgbValue[1]);
  analogWrite(rgbPins[2], rgbrgbValue[2]);
}




buildingState states[LEDS];



void commandHandler(const RGBMessageReceiver &receiver) {

  int commandrgb[3];
  receiver.rgb(commandrgb);
/*
  Serial.println("message received");

  Serial.println(receiver.text());

  Serial.print("status:");
  Serial.println(receiver.status());

  Serial.print("led:");
  Serial.println(int(receiver.led()));
*/
  byte led= receiver.led();


  // set rgb values to be displayed
  states[led].rgbValue[0]= commandrgb[0] % 256;
  states[led].rgbValue[1]= commandrgb[1] % 256;
  states[led].rgbValue[2]= commandrgb[2] % 256;

  states[led].isOn=false;

  switch (receiver.status()) {
  case 'S':
    states[led].currentAction = inactive;
    break;
  case 'B':
    states[led].currentAction = building;
    break;
  case 'P':
    states[led].currentAction = pending;
    break;
  default:
    Serial.println("unknown command");
  }
}

RGBMessageReceiver rgbMessage(commandHandler);

void initState(int led) {

  setArray(states[led].rgbPins ,  JOBS_PINS[led] );
  setArray(states[led].rgbValue,OFF);
  states[led].previousMillis = 0;
  states[led].interval = LED_UPDATE_MS;

  // initial state of the job
  states[led].currentAction  = inactive;

  // jobs initialization
  // TODO must be done somewhere else
  pinMode(states[led].rgbPins[0], OUTPUT);      // sets the digital pin as output
  pinMode(states[led].rgbPins[1], OUTPUT);      // sets the digital pin as output
  pinMode(states[led].rgbPins[2], OUTPUT);      // sets the digital pin as output

}


void setup()
{

  for (int led=0;led<LEDS;led++) 
  {
    initState(led);
  }

  Serial.begin(9600); 
  Serial.println("waiting for messages information"); 


}


void loop()
{
  for (int led=0;led<LEDS;led++) 
  {
    states[led].currentAction(states[led]);
  }

  while (Serial.available() > 0) {
    rgbMessage.accept(Serial.read());
  }

}
